#include "PID.h"
#include "math.h"
float PID_Controller::Adjust()
{
    Error = Target - Current;

    if (abs(Error) < DeadZone)
    {
        Out = 0;
        return Out;
    }

    P_Term = Kp * Error;
		
		integral_e += Error * 0.01f;
		integral_e = output_constrain(integral_e, -I_Term_Max/Ki, I_Term_Max/Ki);

    /* Using Integral separation */
    if (abs(Error) < I_SeparThresh)
    {
        I_Term = Ki * integral_e;
        /*Constarin*/
        I_Term = output_constrain(I_Term, -I_Term_Max, I_Term_Max);
    }
    else
    {
        /*Clear*/
        I_Term = 0;
    }

    float d_err = 0;
    d_err = Error - pre_error;
    D_Term = Kd * d_err;

    pre_error = Error;

    Out = P_Term + I_Term + D_Term;

    /* Constarin */
    Out = output_constrain(Out, -Out_Max, Out_Max);

    return Out;
}

float PID_Controller::output_constrain(float _val,float _min,float _max)
{
    if(_val < _min)
        return _min;
    else if(_val > _max)
        return _max;
    else
        return _val;
}

